home *** CD-ROM | disk | FTP | other *** search
/ Games of Daze / Infomagic - Games of Daze (Summer 1995) (Disc 1 of 2).iso / x2ftp / msdos / math / newmat08 / svd.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1995-01-11  |  5.2 KB  |  179 lines
  1. //$$svd.cpp                           singular value decomposition
  2.  
  3. // Copyright (C) 1991,2,3,4: R B Davies
  4.  
  5. #define WANT_MATH
  6. #define WANT_STREAM
  7.  
  8. #include "include.h"
  9. #include "newmat.h"
  10. #include "newmatrm.h"
  11. #include "precisio.h"
  12.  
  13. static Real pythag(Real f, Real g, Real& c, Real& s)
  14. // return z=sqrt(f*f+g*g), c=f/z, s=g/z
  15. // set c=1,s=0 if z==0
  16. // avoid floating point overflow or divide by zero
  17. {
  18.    if (f==0 && g==0) { c=1.0; s=0.0; return 0.0; }
  19.    Real af = f>=0 ? f : -f;
  20.    Real ag = g>=0 ? g : -g;
  21.    if (ag<af)
  22.       { Real h = g/f; Real sq = sqrt(1.0+h*h); c=1.0/sq; s=h/sq; return sq*f; }
  23.    else
  24.       { Real h = f/g; Real sq = sqrt(1.0+h*h); s=1.0/sq; c=h/sq; return sq*g; }
  25. }
  26.  
  27. void SVD(const Matrix& A, DiagonalMatrix& Q, Matrix& U, Matrix& V,
  28.    Boolean withU, Boolean withV)
  29. // from Wilkinson and Reinsch: "Handbook of Automatic Computation"
  30. {
  31.    Tracer trace("SVD");
  32.    Real eps = FloatingPointPrecision::Epsilon();
  33.    Real tol = FloatingPointPrecision::Minimum()/eps;
  34.  
  35.    int m = A.Nrows(); int n = A.Ncols();
  36.    if (m<n) 
  37.       Throw(ProgramException("Want no. Rows >= no. Cols", A));
  38.  
  39.    U = A; Real g = 0.0; Real f,h; Real x = 0.0;
  40.    RowVector E(n); RectMatrixRow EI(E,0); Q.ReDimension(n);
  41.    RectMatrixCol UCI(U,0); RectMatrixRow URI(U,0,1,n-1);
  42.  
  43.    for (int i=0; i<n; i++)
  44.    {
  45.       EI.First() = g; Real ei = g; EI.Right(); Real s = UCI.SumSquare();
  46.       if (s<tol) Q.element(i) = 0.0;
  47.       else
  48.       {
  49.      f = UCI.First(); g = -sign(sqrt(s), f); h = f*g-s; UCI.First() = f-g;
  50.      Q.element(i) = g; RectMatrixCol UCJ = UCI; int j=n-i;
  51.      while (--j) { UCJ.Right(); UCJ.AddScaled(UCI, (UCI*UCJ)/h); }
  52.       }
  53.  
  54.       s = URI.SumSquare();
  55.       if (s<tol) g = 0.0;
  56.       else
  57.       {
  58.      f = URI.First(); g = -sign(sqrt(s), f); URI.First() = f-g;
  59.      EI.Divide(URI,f*g-s); RectMatrixRow URJ = URI; int j=m-i;
  60.      while (--j) { URJ.Down(); URJ.AddScaled(EI, URI*URJ); }
  61.       }
  62.  
  63.       Real y = fabs(Q.element(i)) + fabs(ei); if (x<y) x = y;
  64.       UCI.DownDiag(); URI.DownDiag();
  65.    }
  66.  
  67.    if (withV)
  68.    {
  69.       V.ReDimension(n,n); V = 0.0; RectMatrixCol VCI(V,n,n,0);
  70.       for (i=n-1; i>=0; i--)
  71.       {
  72.      URI.UpDiag(); VCI.Left();
  73.      if (g!=0.0)
  74.      {
  75.         VCI.Divide(URI, URI.First()*g); int j = n-i;
  76.         RectMatrixCol VCJ = VCI;
  77.         while (--j) { VCJ.Right(); VCJ.AddScaled( VCI, (URI*VCJ) ); }
  78.      }
  79.      VCI.Zero(); VCI.Up(); VCI.First() = 1.0; g=E.element(i);
  80.       }
  81.    }
  82.  
  83.    if (withU)
  84.    {
  85.       for (i=n-1; i>=0; i--)
  86.       {
  87.      UCI.UpDiag(); g = Q.element(i); URI.Reset(U,i,i+1,n-i-1); URI.Zero();
  88.          if (g!=0.0)
  89.          {
  90.         h=UCI.First()*g; int j=n-i; RectMatrixCol UCJ = UCI;
  91.         while (--j)
  92.             {
  93.            UCJ.Right(); UCI.Down(); UCJ.Down(); Real s = UCI*UCJ;
  94.                UCI.Up(); UCJ.Up(); UCJ.AddScaled(UCI,s/h);
  95.             }
  96.         UCI.Divide(g);
  97.          }
  98.          else UCI.Zero();
  99.          UCI.First() += 1.0;
  100.       }
  101.    }
  102.  
  103.    eps *= x;
  104.    for (int k=n-1; k>=0; k--)
  105.    {
  106.       Real z; Real y; int limit = 50; int l;
  107.       while (limit--)
  108.       {
  109.      Real c=0.0; Real s=1.0; int i; int l1=k; Boolean tfc=FALSE;
  110.      for (l=k; l>=0; l--)
  111.      {
  112. //          if (fabs(E.element(l))<=eps) goto test_f_convergence;
  113.         if (fabs(E.element(l))<=eps) { tfc=TRUE; break; }
  114.         if (fabs(Q.element(l-1))<=eps) { l1=l; break; }
  115.      }
  116.          if (!tfc)
  117.          {
  118.         l=l1; l1=l-1;
  119.         for (i=l; i<=k; i++)
  120.         {
  121.            f = s * E.element(i); E.element(i) *= c;
  122. //             if (fabs(f)<=eps) goto test_f_convergence;
  123.            if (fabs(f)<=eps) break;
  124.                Real fh,gh;
  125.            g = Q.element(i); h = pythag(f,g,fh,gh); Q.element(i) = h;
  126.            if (withU)
  127.            {
  128.               RectMatrixCol UCI(U,i); RectMatrixCol UCJ(U,l1);
  129.               ComplexScale(UCI, UCJ, gh, -fh);
  130.                }
  131.             }
  132.      }
  133. //    test_f_convergence: z = Q.element(k); if (l==k) goto convergence;
  134.      z = Q.element(k);  if (l==k) break;
  135.  
  136.      x = Q.element(l); y = Q.element(k-1);
  137.      g = E.element(k-1); h = E.element(k);
  138.      f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2*h*y);
  139.          if (f>1)         g = f * sqrt(1 + square(1/f));
  140.          else if (f<-1)   g = -f * sqrt(1 + square(1/f));
  141.          else             g = sqrt(f*f + 1);
  142.      f = ((x-z)*(x+z) + h*(y / ((f<0.0) ? f-g : f+g)-h)) / x;
  143.  
  144.      c = 1.0; s = 1.0;
  145.      for (i=l+1; i<=k; i++)
  146.      {
  147.         g = E.element(i); y = Q.element(i); h = s*g; g *= c;
  148.         z = pythag(f,h,c,s); E.element(i-1) = z;
  149.         f = x*c + g*s; g = -x*s + g*c; h = y*s; y *= c;
  150.         if (withV)
  151.         {
  152.            RectMatrixCol VCI(V,i); RectMatrixCol VCJ(V,i-1);
  153.            ComplexScale(VCI, VCJ, c, s);
  154.         }
  155.         z = pythag(f,h,c,s); Q.element(i-1) = z;
  156.         f = c*g + s*y; x = -s*g + c*y;
  157.         if (withU)
  158.         {
  159.            RectMatrixCol UCI(U,i); RectMatrixCol UCJ(U,i-1);
  160.            ComplexScale(UCI, UCJ, c, s);
  161.         }
  162.      }
  163.      E.element(l) = 0.0; E.element(k) = f; Q.element(k) = x;
  164.       }
  165.       if (l!=k) { Throw(ConvergenceException(A)); }
  166. // convergence:
  167.       if (z < 0.0)
  168.       {
  169.      Q.element(k) = -z;
  170.      if (withV) { RectMatrixCol VCI(V,k); VCI.Negate(); }
  171.       }
  172.    }
  173. }
  174.  
  175. void SVD(const Matrix& A, DiagonalMatrix& D)
  176. { Matrix U; SVD(A, D, U, U, FALSE, FALSE); }
  177.  
  178.  
  179.